/**
 * 
 */
package it.unicam.cs.cosy.bioshape.core.collisiondetection.narrowphase;

import it.unicam.cs.cosy.bioshape.core.behaviours.ActiveSite;
import it.unicam.cs.cosy.bioshape.core.components.EntityPair;
import it.unicam.cs.cosy.bioshape.core.components.ShapedEntity;
import it.unicam.cs.cosy.bioshape.core.components.WallEntity;

import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;

import javax.vecmath.Matrix3d;
import javax.vecmath.Matrix4d;
import javax.vecmath.Vector3d;

import vclip.DistanceReport;
import vclip.ClosestFeaturesHT;

/**
 * @author Luca Tesei
 * 
 */
public class VClipCollisionDetector implements NarrowPhase {

    // /*
    // * Directions in which the binary search of the first time of contact
    // could
    // * be going at a certain moment
    // */
    // private static final int BACKWARD_DIRECTION = 0;
    //
    // private static final int FORWARD_DIRECTION = 1;

    private List<EntityPair> pairs;

    private ClosestFeaturesHT ht;

    /**
     * Generate a VClip Collision Detector Module
     */
    public VClipCollisionDetector() {
        this.pairs = null;
        this.ht = null; // new ClosestFeaturesHT();
    }

    /**
     * If the distances between all the colliding pairs of shapes at the middle
     * point of the current interval of ftc search are positive and less than
     * POSITIVE_DISTANCE_TOLERANCE then the middle point is taken as the first
     * time of contact and the search will end
     */
    private static final double POSITIVE_DISTANCE_TOLERANCE = 1E-5;

    /**
     * If the search interval of the first time of contact has length less than
     * EPSILON_INTERVAL then the first time of contact is considered to be equal
     * to the middlePoint of the interval
     */
    private static final double EPSILON_INTERVAL = 1E-5;

//    /**
//     * To escape from a left cycle, apply this "fake" ftc
//     */
//    private static final double EPSILON_ESCAPE = 0.1;

    // /**
    // * If the lowest distance between two shapes is less than or equal to
    // * EPSILON_DISTANCE then the two shapes are considered touching
    // */
    // private static final double EPSILON_DISTANCE = 0;

    /**
     * Search for the first time of contact. The fundamental hypothesis of this
     * search algorithm is that the given pairs are NOT penetrating at current
     * time, i.e., at time zero of the search interval
     * 
     * @param pairs
     *            a list of possible colliding pairs, presumably calculated by
     *            the broad phase
     * @param delta
     *            the length of the interval for which the first time of contact
     *            is searched for, that is to say, the interval [0, delta]
     * @return the first time of contact among the possible colliding pairs OR
     *         Double.POSITIVE_INFINITY if none of the given pairs is colliding
     *         in the interval [0,delta]
     */
    private static double getFirstTimeOfContact(List<EntityPair> pairs,
            double delta, ClosestFeaturesHT ht) {
        int iterationCount = 0;
        int leftCount = 0;
        /*
         * The search for ftc could be called even if the broad phase detected
         * no possible colliding pairs, in this case the ftc is "infinite", by
         * convention
         */
        if (pairs.size() == 0)
            return Double.POSITIVE_INFINITY;
        // /*
        // * Preliminary check - no pair can penetrate at time zero, this is a
        // * fundamental hypothesis of the implemented search algorithm
        // */
        // for (EntityPair pair : pairs) {
        // DistanceReport drep = new DistanceReport();
        // // drep.setFeaturePromotion(true, Math.toRadians(10));
        // // drep.setMaxPairDistance(EPSILON_DISTANCE); // maximal
        // // distance
        // // drep.setMaxClosePairs(1); // closest features
        // double distance = getDistanceAfterTime(0, pair, drep, ht);
        // if (distance < 0)
        // throw new IllegalStateException(
        // "Penetrating pair at the beginning of iteration! Error! Pair = "
        // + pair);
        // }
        /*
         * Search for ftc can normally start. Determine the first values for the
         * left, middle and right point of the search interval
         */
        double left = 0;
        double right = delta;
        double middle = delta / 2;
        /*
         * Only at the first iteration of the searching any distance must be
         * calculated
         */
        boolean firstCycle = true;
        /*
         * if the last right limit of the interval is equal to the current right
         * limit then vclip call for the right limit can be avoided
         */
        boolean useLastRight = false;
        /*
         * if the last middle point of the interval is equal to the current
         * right limit then vclip call for the right limit can be avoided
         */
        boolean useLastMiddleAsRight = false;
        /*
         * Control of the cycle
         */
        boolean done = false;
        /*
         * the pairs are divided into those colliding in the right-half of the
         * interval including middle - i.e., [middle, right] -, those colliding
         * in the left-half of the interval - i.e., [left,middle) - and those
         * not colliding at all in the current interval (this can happen only at
         * the first iteration). In any point x of the interval, if the distance
         * between two shapes at x is less than 0, then they are penetrating
         */
        List<EntityPair> leftMiddleCollidingPairs = new ArrayList<EntityPair>();
        List<EntityPair> middleRightCollidingPairs = new ArrayList<EntityPair>();
        System.out.print("FTC-iteration:");
        while (!done) {
            iterationCount++;
            System.out.print(" " + iterationCount);
            /*
             * done will never become true, the cycle exits when an acceptable
             * ftc has been determined OR an IllegalStateException is raised
             * because this is not possible.
             */
            leftMiddleCollidingPairs.clear();
            middleRightCollidingPairs.clear();
            /*
             * Internal cycle to analyze all possibly colliding pairs
             */
            Iterator<EntityPair> iterator = pairs.iterator();
            while (iterator.hasNext()) {
                EntityPair pair = iterator.next();
                /*
                 * save current limits in the pair
                 */
                pair.setLeft(left);
                pair.setMiddle(middle);
                pair.setRight(right);
                DistanceReport drepMiddle = null;
                DistanceReport drepRight = null;
                double distanceMiddle = 0;
                double distanceRight = 0;
                /*
                 * Calculate the distances of the two shapes at time middle and
                 * at time right, possibly saving a call to vclip if one of the
                 * last values can be used
                 */
                if (firstCycle) {
                    drepMiddle = new DistanceReport();
                    // drepMiddle.setFeaturePromotion(true, Math.toRadians(10));
                    // drep.setMaxPairDistance(EPSILON_DISTANCE); // maximal
                    // distance
                    // drepMiddle.setMaxClosePairs(1); // closest features
                    distanceMiddle = getDistanceAfterTime(middle, pair,
                            drepMiddle, ht);
                    pair.setDistanceMiddle(distanceMiddle);
                    pair.setDistanceReportMiddle(drepMiddle);
                    drepRight = new DistanceReport();
                    // drepRight.setFeaturePromotion(true, Math.toRadians(10));
                    // drep.setMaxPairDistance(EPSILON_DISTANCE); // maximal
                    // distance
                    // drepRight.setMaxClosePairs(1); // closest features
                    distanceRight = getDistanceAfterTime(right, pair,
                            drepRight, ht);
                    pair.setDistanceRight(distanceRight);
                    pair.setDistanceReportRight(drepRight);
                } else {
                    if (useLastRight) {
                        distanceRight = pair.getDistanceRight();
                        drepRight = pair.getDistanceReportRight();
                        drepMiddle = new DistanceReport();
                        // drepMiddle
                        // .setFeaturePromotion(true, Math.toRadians(10));
                        // drep.setMaxPairDistance(EPSILON_DISTANCE); //
                        // maximal
                        // distance
                        // drepMiddle.setMaxClosePairs(1); // closest features
                        distanceMiddle = getDistanceAfterTime(middle, pair,
                                drepMiddle, ht);
                        pair.setDistanceMiddle(distanceMiddle);
                        pair.setDistanceReportMiddle(drepMiddle);
                    } else {
                        if (useLastMiddleAsRight) {
                            distanceRight = pair.getDistanceMiddle();
                            drepRight = pair.getDistanceReportMiddle();
                            pair.setDistanceReportRight(drepRight);
                            pair.setDistanceRight(distanceRight);
                            drepMiddle = new DistanceReport();
                            // drepMiddle.setFeaturePromotion(true,
                            // Math.toRadians(10));
                            // drep.setMaxPairDistance(EPSILON_DISTANCE); //
                            // maximal
                            // distance
                            // drepMiddle.setMaxClosePairs(1); // closest
                            distanceMiddle = getDistanceAfterTime(middle, pair,
                                    drepMiddle, ht);
                            pair.setDistanceMiddle(distanceMiddle);
                            pair.setDistanceReportMiddle(drepMiddle);
                        }
                    }
                }
                /*
                 * Analyze all possible cases
                 * 
                 * Case #1: the shapes penetrate at the right limit of the
                 * interval and they do not at the middle point of the interval.
                 * Thus, the contact point must be in the right half of the
                 * current interval. The pair is moved in the corresponding list
                 * (middleRight) and temporarily removed from pairs
                 */
                if (distanceRight <= 0 && distanceMiddle > 0) {
                    middleRightCollidingPairs.add(pair);
                    iterator.remove();
                }
                /*
                 * Case #2: the shapes penetrate at the middle point of the
                 * interval, but not anymore at the right limit. This can happen
                 * when there is rotation: even if they get closer by
                 * translation the rotation makes them to not penetrate anymore.
                 * The real collision must have happened in the left half of the
                 * interval. The pair is moved in the list leftMiddle and
                 * temporarily removed from pairs.
                 */
                if (distanceRight > 0 && distanceMiddle <= 0) {
                    leftMiddleCollidingPairs.add(pair);
                    iterator.remove();
                }
                /*
                 * Case #3 and #4: the shapes do not penetrate at the middle of
                 * the interval, nor they do at the right limit of the interval.
                 * Thus, they will not collide in the current interval and in
                 * any of its sub-intervals. The pair is removed definitely from
                 * the colliding pairs. They collision, if any, will be detected
                 * in the following iteration(s).
                 */
                if (distanceRight > 0 && distanceMiddle > 0) {
                    iterator.remove();
                }
                /*
                 * Case #5 and #6: the shapes penetrate both at the middle and
                 * at the right limit of the interval. Thus, since the
                 * hypothesis is that they were not colliding at time zero, then
                 * the collision must occur in the left half of the interval.
                 * The pair is moved in the list leftMiddle and temporarily
                 * removed from pairs.
                 */
                if (distanceRight <= 0 && distanceMiddle <= 0) {
                    leftMiddleCollidingPairs.add(pair);
                    iterator.remove();
                }
            } // end of internal while
            /*
             * At this point no pairs are left in 'pairs'
             */
            if (leftMiddleCollidingPairs.size() != 0) {
                System.out.print(" left ");
                leftCount++;
                /*
                 * There are pairs colliding in the left half of the interval,
                 * so the ones colliding in the right half will be definitely
                 * lost and the limits of the interval are rewritten to its left
                 * half part.
                 */
                /*
                 * It can happen, anyway, that a cycle
                 * starts of all left iterations which goes on for a lot of
                 * iterations. In this case, we return the left limit of the interval,
                 * which could be zero, but the collision response should resolve the problem
                 */
                //double ftc = 0;
                if ((leftCount == iterationCount)
                        && (right - left) < EPSILON_INTERVAL 
                        // && allPairsNoWall(leftMiddleCollidingPairs)
                        ) {
                    /*
                     * Ending Here! The ftc is the left point of the interval.
                     * Upgrading the information associated to the pairs
                     */
                    for (EntityPair p : leftMiddleCollidingPairs) {
                        DistanceReport drep = new DistanceReport();
                        double distance = getDistanceAfterTime(left, p,
                                drep, ht);
                        p.setDistance(distance);
                        p.setDistanceReport(drep);
                    }
                    /*
                     * Put everything back in 'pairs'
                     */
                    pairs.addAll(leftMiddleCollidingPairs);
                    /*
                     * Returning the middle point as first time of contact
                     */
                    System.out.println("Found !!! FTC = " + left);
                    return left;
                }
                /*
                 * Otherwise proceed normally
                 */
                right = middle;
                middle = left + (right - left) / 2;
                /*
                 * Update control variables for saving calls to vclip
                 */
                firstCycle = false;
                useLastMiddleAsRight = true;
                useLastRight = false;
                /*
                 * Set the remaining pairs to work on
                 */
                pairs.addAll(leftMiddleCollidingPairs);
            } else if (middleRightCollidingPairs.size() != 0) {
                System.out.print(" right ");
                /*
                 * There are no pairs colliding in the left half of the
                 * interval, but there are ones colliding in the right half.
                 * Thus, all these will be retained to continue the search and
                 * the limits of the interval are rewritten to its right half
                 * part.
                 */
                /*
                 * This could be the end of the search! If ((all pairs in
                 * middleRightCollidingPairs have a positive distance AND this
                 * distance is less than POSITIVE_DISTANCE_TOLERANCE) OR if the
                 * length of the last interval of search is less than
                 * EPSILON_INTERVAL) then we can stop here! Starting the search
                 * for at least one pair in which the distance is not positive
                 * OR it is positive, but greater than or equal to the tolerance
                 */
                boolean found = false;
                int i = 0;
                while (i < middleRightCollidingPairs.size() && !found) {
                    double d = middleRightCollidingPairs.get(i)
                            .getDistanceMiddle();
                    if (d <= 0 || d >= POSITIVE_DISTANCE_TOLERANCE)
                        found = true;
                    else
                        i++;
                }
                if (!found || (right - left) < EPSILON_INTERVAL) {
                    /*
                     * Ending Here! The ftc is the middle point of the interval.
                     * Upgrading the information associated to the pairs
                     */
                    for (EntityPair p : middleRightCollidingPairs) {
                        p.setDistance(p.getDistanceMiddle());
                        p.setDistanceReport(p.getDistanceReportMiddle());
                    }
                    /*
                     * Put everything back in 'pairs'
                     */
                    pairs.addAll(middleRightCollidingPairs);
                    /*
                     * Returning the middle point as first time of contact
                     */
                    System.out.println("Found !!! FTC = " + middle
                            + " by distance? " + !found);
                    return middle;
                }
                /*
                 * Otherwise, go on normally
                 */
                left = middle;
                middle = left + (right - left) / 2;
                /*
                 * Update control variables for saving calls to vclip
                 */
                firstCycle = false;
                useLastMiddleAsRight = false;
                useLastRight = true;
                /*
                 * Set the pairs back to 'pairs' to continue working on them
                 */
                pairs.addAll(middleRightCollidingPairs);
            } else {
                /*
                 * There are no pairs colliding in the whole interval! This can
                 * happen only at the first iteration of the external while. In
                 * this case, by convention, the Double.POSITIVE_INFINITE
                 * constant is returned.
                 */
                System.out
                        .println("\nNobody really colliding in this interval!");
                return Double.POSITIVE_INFINITY;
            }
            // /*
            // * This could happen when the external while continue to be
            // executed
            // * (indefinitely towards left) until the interval becomes of only
            // * one point or null. This should normally never happen, but a
            // * control is put for safety.
            // */
            // if (right - left <= 0) {
            // // Should never reach this point
            // throw new IllegalStateException(
            // "Impossible to find a ftc in which no pair is penetrating; left = "
            // + left + " right = " + right);
            // }
        } // end external while
        return Double.POSITIVE_INFINITY;
    }

    private static boolean allPairsNoWall(
            List<EntityPair> pairs) {
        for (EntityPair p : pairs){
            if (p.getE1().getClass() == WallEntity.class)
                return false;
            if (p.getE2().getClass() == WallEntity.class)
                return false;
        }
        return true;
    }

    /**
     * @param t
     * @param pair
     * @param drep
     * @param ht
     * @return
     */
    public static double getDistanceAfterTime(double t, EntityPair pair,
            DistanceReport drep, ClosestFeaturesHT ht) {
        // Backup of current transformation matrices
        ShapedEntity e1 = (ShapedEntity) pair.getE1();
        Matrix4d backupE1M = new Matrix4d(e1.getTransform());
        ShapedEntity e2 = (ShapedEntity) pair.getE2();
        Matrix4d backupE2M = new Matrix4d(e2.getTransform());
        // Move the shapes to time rightLimit
        e1.move(t);
        e2.move(t);
        // Calculate transformation matrix From e2's frame to e1's frame
        Matrix4d r2r1 = new Matrix4d();
        Matrix4d e1MInverse = new Matrix4d();
        inverseWithTranspose(e1.getTransform(), e1MInverse);
        r2r1.mul(e1MInverse, e2.getTransform());
        // Put both the entities back in the origin
        e1.getTransform().setIdentity();
        e2.getTransform().setIdentity();
        double distance = e1.vclip(drep, e2, r2r1, -1, ht);
        // System.out.println("e1 class = " + e1.getClass().getName());
        // System.out.println("e2 class = " + e2.getClass().getName());
        // System.out.println("nrml as is = " + drep.getClosestPair().nrml);
        // drep.transformFirstPoints(backupE1M);
        // System.out.println("nrml transformed m1 = "
        // + drep.getClosestPair().nrml);
        // Restore original e1's and e2's transformation matrices
        e1.getTransform().set(backupE1M);
        e2.getTransform().set(backupE2M);
        return distance;
    }

    /**
     * Calculate the transformation matrix that transforms from a reference
     * frame 2 to a reference frame 1, both frames given as transformation
     * matrices as well.
     * 
     * @param referenceFrame1TransformationMatrix
     *            the transformation matrix transforming to reference frame 1,
     *            left unchanged
     * @param referenceFrame2TransformationMatrix
     *            the transformation matrix transforming to reference frame 2,
     *            left unchanged
     * @param r2r1TransformationMatrix
     *            the transformation matrix transforming from reference frame 2
     *            to reference frame 1
     */
    public static void fromReferenceFrame2ToReferenceFrame1(
            Matrix4d referenceFrame1TransformationMatrix,
            Matrix4d referenceFrame2TransformationMatrix,
            Matrix4d r2r1TransformationMatrix) {
        Matrix4d inverse2 = new Matrix4d();
        inverseWithTranspose(referenceFrame2TransformationMatrix, inverse2);
        r2r1TransformationMatrix.mul(referenceFrame1TransformationMatrix,
                inverse2);
    }

    /**
     * Calculate the inverse of a transformation matrix 4x4 (composed of a
     * rotational part rot, which is a 3x3 matrix, a translational part t, which
     * is a 3 component vector, and the last row equal to 0 0 0 1) using the
     * property that this is always equal to the 4x4 matrix composed by the
     * transpose of rot, and the transformation of t by the negation of the
     * transpose of rot: t' = -rot^T * t. The last row is again 0, 0, 0, 1
     * 
     * @param original
     *            the original matrix to invert, left unchanged
     * @param inverted
     *            the inverted matrix
     */
    public static void inverseWithTranspose(Matrix4d original, Matrix4d inverted) {
        Matrix3d originalTransposeRot = new Matrix3d();
        original.get(originalTransposeRot);
        originalTransposeRot.transpose();
        inverted.setRotationScale(originalTransposeRot);
        inverted.m30 = 0;
        inverted.m31 = 0;
        inverted.m32 = 0;
        inverted.m33 = 1;
        Vector3d translationOriginal = new Vector3d();
        original.get(translationOriginal);
        originalTransposeRot.negate();
        originalTransposeRot.transform(translationOriginal);
        inverted.setTranslation(translationOriginal);
    }

    @Override
    public ActiveSite[] getContactSurfaces() {
        // TODO Auto-generated method stub
        return null;
    }

    @Override
    public List<EntityPair> getPairs() {
        return pairs;
    }

    @Override
    public double getFTC(List<EntityPair> pairs, double delta)
            throws SearchingErrorException {
        this.pairs = pairs;
        // the object pairs is manipulated in the static method, it will hold
        // the relevant elements after the function has returned
        return VClipCollisionDetector.getFirstTimeOfContact(this.pairs, delta,
                this.ht);
    }
}
